package org.pageley.games.sanctuary.domain.environment.pathfinding;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;

import org.pageley.games.sanctuary.domain.Direction;
import org.pageley.games.sanctuary.domain.Location;

public class AStarAlgorithm implements PathFindingAlgorithm {

	private DistanceHeuristic heuristic;
	
	public AStarAlgorithm(DistanceHeuristic heuristic) {
		this.heuristic = heuristic;
	}
	
	@Override
	public List<Location> findPath(Location start, Location end,
			Collection<Location> nodes) {
		List<Location> result = new ArrayList<Location>();
		if(nodes.contains(end)) {
			Collection<Location> evaluatedNodes = new HashSet<Location>();
			Collection<Location> tentativeNodes = new HashSet<Location>();
			for (Location location : nodes) {
				tentativeNodes.add(location);
			}
			Map<Location, Double> distanceFromStart = new HashMap<Location, Double>();
			Map<Location, Double> heuristicDistanceToGoal = new HashMap<Location, Double>();
			Map<Location, Double> estimatedTotalDistanceToGoal = new HashMap<Location, Double>();
			Map<Location, Location> trail = new HashMap<Location, Location>();
	
			distanceFromStart.put(start, 0.0);
			heuristicDistanceToGoal.put(start, heuristic.estimateDistance(start, end));
			estimatedTotalDistanceToGoal.put(start, heuristicDistanceToGoal
					.get(start));
			while (!tentativeNodes.isEmpty()) {
				Location locationToTry = null;
				Double minDistance = Double.MAX_VALUE;
				for (Location l : tentativeNodes) {
					Double d = estimatedTotalDistanceToGoal.get(l);
					if (d != null && d < minDistance) {
						locationToTry = l;
						minDistance = d;
					}
				}
				if (locationToTry == null) {
					break;
				}
				if (locationToTry.equals(end)) {
					result = reconstructPath(trail, end);
				}
				tentativeNodes.remove(locationToTry);
				evaluatedNodes.add(locationToTry);
				for (Location l : getNeighbors(locationToTry, nodes)) {
					if (!evaluatedNodes.contains(l)) {
						Location location = l;
						Double tentativeDistanceFromStart = distanceFromStart.get(locationToTry)
								+ locationToTry.distanceTo(location);
						boolean tentativeBetter = false;
						if (!tentativeNodes.contains(l)) {
							tentativeNodes.add(l);
							heuristicDistanceToGoal.put(location, heuristic.estimateDistance(location, end));
							tentativeBetter = true;
						} else if (distanceFromStart.get(location) == null
								|| tentativeDistanceFromStart < distanceFromStart.get(location)) {
							tentativeBetter = true;
						}
						if (tentativeBetter) {
							trail.put(location, locationToTry);
							distanceFromStart.put(location,
									tentativeDistanceFromStart);
							Double heuristicDistance = heuristicDistanceToGoal
									.get(location);
							if (heuristicDistance == null) {
								heuristicDistance = 0.0;
							}
							estimatedTotalDistanceToGoal.put(location,
									distanceFromStart.get(location)
											+ heuristicDistance);
						}
					}
				}
			}
		}
		return result;
	}

	private List<Location> getNeighbors(Location location, Collection<Location> allLocations) {
		List<Location> neighbors = new ArrayList<Location>();
		for (Direction direction : Direction.values()) {
			Location l = location.locationIn(direction);
			if (allLocations.contains(location)) {
				neighbors.add(l);
			}
		}
		return neighbors;
	}

	private List<Location> reconstructPath(Map<Location, Location> trail,
			Location goal) {
		List<Location> path = new ArrayList<Location>();
		if (trail.get(goal) != null) {
			path = reconstructPath(trail, trail.get(goal));
			path.add(goal);
		}
		return path;
	}
}
